#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include "std_msgs/msg/float64.hpp"
#include <std_msgs/msg/float32_multi_array.hpp>
#include "std_msgs/msg/int32.hpp"

class Ros2Ros : public rclcpp::Node
{
public:
    Ros2Ros() : Node("ros2ros")
    {
        sub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
        rclcpp::SubscriptionOptions options_sub;
        options_sub.callback_group = sub_callback_group_;

        pub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        // 订阅点云话题
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/line_point", 10,
            std::bind(&Ros2Ros::pointCloudCallback,
                      this, std::placeholders::_1),
            options_sub);
        // 向constyle4发布感知的信息（倒U）
        to_do_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("float_array_topic", 5);
        // 大阀门
        state = this->create_subscription<std_msgs::msg::Int32>(
            "/valve_state", 10,
            std::bind(&Ros2Ros::valveCallback,
                      this, std::placeholders::_1),
            options_sub);
        state_pub_ = this->create_publisher<std_msgs::msg::Int32>("big_valve_state", 5);
        // 小阀门 1
        state_1 = this->create_subscription<std_msgs::msg::Int32>(
            "/valve_1_state", 10,
            std::bind(&Ros2Ros::valveCallback_1,
                      this, std::placeholders::_1),
            options_sub);
        state1_pub_ = this->create_publisher<std_msgs::msg::Int32>("valve_state_1", 5);
        // 小阀门 2
        state_2 = this->create_subscription<std_msgs::msg::Int32>(
            "/valve_2_state", 10,
            std::bind(&Ros2Ros::valveCallback_2,
                      this, std::placeholders::_1),
            options_sub);
        state2_pub_ = this->create_publisher<std_msgs::msg::Int32>("valve_state_2", 5);
        // 二号关节压力

        // 三号关节压力

        // 一号关节转动角度

        // 二号关节转动角度

        // 三号关节转动角度

        // 四号关节转动角度

        // 末端位姿
    }

private:
    int valve_state = 0; // 用于存储阀门状态的变量
    int state_first = 0;
    int state_second = 0;

    rclcpp::CallbackGroup::SharedPtr sub_callback_group_;
    rclcpp::CallbackGroup::SharedPtr pub_callback_group_;
    // U形轨迹信息
    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        // 传去左侧、右侧及高度信息
        float x_right;
        float x_left;
        float z_high;
        auto merged_pose = std_msgs::msg::Float32MultiArray();
        // 创建定时器，控制输出频率
        static rclcpp::Rate rate(10); // 设置输出频率为1Hz
        rate.sleep();

        // 迭代器访问点云数据
        sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");

        // 遍历点云中的每个点
        for (size_t i = 0; i < msg->width * msg->height; ++i)
        {
            std::cout << "来自ROS2的消息\n";
            RCLCPP_INFO(this->get_logger(), "Point %zu: (%f, %f, %f)", i, *iter_x, *iter_y, *iter_z);
            ++iter_x;
            ++iter_y;
            ++iter_z;
        }
        // 赋值并发布
        merged_pose.data[0] = x_right;
        merged_pose.data[1] = x_left; // 此处的值应该是负的
        merged_pose.data[2] = z_high;
        to_do_->publish(merged_pose);
    }
    // 大阀门的开关量
    void valveCallback(const std_msgs::msg::Int32::SharedPtr state_msg)
    {
        auto valveCallback_value = std_msgs::msg::Int32();
        valve_state = state_msg->data;
        switch (valve_state)
        {
        case 0:
            valveCallback_value.data = 0;
            state_pub_->publish(valveCallback_value);
            break;

        case 1:
            valveCallback_value.data = 1;
            state_pub_->publish(valveCallback_value);
            break;

        default:
            break;
        }
    }
    // 小阀门 1
    void valveCallback_1(const std_msgs::msg::Int32 state_msg)
    {
        auto valveCallback_1_value = std_msgs::msg::Int32();
        state_first = state_msg.data;
        switch (state_first)
        {
        case 0:
            valveCallback_1_value.data = 0;
            state1_pub_->publish(valveCallback_1_value);
            break;
        case 1:
            valveCallback_1_value.data = 1;
            state1_pub_->publish(valveCallback_1_value);
            break;
        case 2:
            valveCallback_1_value.data = 2;
            state1_pub_->publish(valveCallback_1_value);
            break;
        case 3:
            valveCallback_1_value.data = 3;
            state1_pub_->publish(valveCallback_1_value);
            break;
        case 4:
            valveCallback_1_value.data = 4;
            state1_pub_->publish(valveCallback_1_value);
            break;
        }
    }
    // 小阀门 2
    void valveCallback_2(const std_msgs::msg::Int32 state_msg)
    {
        auto valveCallback_2_value = std_msgs::msg::Int32();
        state_second = state_msg.data;
        switch (state_second)
        {
        case 0:
            valveCallback_2_value.data = 0;
            state2_pub_->publish(valveCallback_2_value);
            break;
        case 1:
            valveCallback_2_value.data = 1;
            state2_pub_->publish(valveCallback_2_value);
            break;
        case 2:
            valveCallback_2_value.data = 2;
            state2_pub_->publish(valveCallback_2_value);
            break;
        case 3:
            valveCallback_2_value.data = 3;
            state2_pub_->publish(valveCallback_2_value);
            break;
        case 4:
            valveCallback_2_value.data = 4;
            state2_pub_->publish(valveCallback_2_value);
            break;
        }
    }
    //
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr to_do_;
    // 大阀门
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr state_pub_;
    // 小阀门 1
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_1;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr state1_pub_;
    // 小阀门 2
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_2;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr state2_pub_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Ros2Ros>();
    // 使用多线程执行器
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}